#include "pid.h"
void pid_init(PID *pid, uint8_t KP, uint8_t KI, uint8_t KD, uint8_t MAX_Integral, uint8_t MAX_Output)
{
	pid.kp = KP;
	pid.ki = KI;
	pid.kd = KD;
	pid.max_Integral = MAX_Integral;
	pid.max_Output = MAX_Output;
}
void pid_cal(PID *pid, uint8_t Target, uint8_t Callback)
{
	pid.Last_Error = pid.Error;
	pid.Error = pid.Error;
	uint8_t dout = (pid.Error - pid.Last_Error) * pid.kd;
	uint8_t pout = pid.Error * pid.kp;
	pid.Integral += pid.Error * pid.ki;
	if(pid.Integral > pid.Max_Integral)
	{
		pid.Integral = pid.Max_Integral;
	}
	if(pid.Integral < (0 - pid.Max_Integral))
	{
		pid.Integral = (0 - pid.Max_Integral);
	}
	pid.Output = pout + dout + pid.Integral;
	if(pid.Output > pid.Max_Output)
	{
		pid.Output = pid.Max_Output;
	}
	if(pid.Output < (0 - pid.Max_Output))
	{
		pid.Output = (0 - pid.Max_Output);
	}  
	
	// pid.Integral < (0 - pid.Max_Integral)
}
